#include "ControlLegUpdown.h"


using namespace gazebo;

//PID初始化函数
void PIDControler::PID_init(double p,double i,double d,double expect)
{
    P = p;
    I = i;
    D = d;
    Expect = expect;

    Error = 0.0;
    Integration = 0.0;
    Error_last = 0.0;

}
    //PID公式
double PIDControler::PID_calc(double feedback)
{
        double p_part,i_part,d_part;
        double output;

        Error_last = Error;
        Error = feedback - Expect;
        Integration += Error;

        p_part = P * Error;
        i_part = I * Integration;
        d_part = D * (Error - Error_last);

        //积分限幅

        output = p_part + i_part + d_part;
        //输出限幅

        return output;
    }


